#ifndef WALKAWARE_KINECT_DEVICE_H
#define WALKAWARE_KINECT_DEVICE_H

#include <assert.h>
#include <memory.h>

#include <libfreenect_sync2.h>

#include <QImage>

#include <WalkaWare/Shared/Vec3.h>
#include <WalkaWare/Shared/RawDepthMap.h>

namespace WalkaWare
{

namespace Kinect
{

class Device
{
	public:

		typedef void   BaseType;
		typedef Device ThisType;

		Device(void)
			: devIndex           (0)
			, rgbLastTimestamp   (0)
			, depthLastTimestamp (0)
			, accelLastTimestamp (0)
		{
			;
		}

		virtual ~Device(void)
		{
			;
		}

		void setDeviceIndex(int index)
		{
			this->devIndex = index;
		}

		int deviceIndex(void) const
		{
			return this->devIndex;
		}

		bool acquireRgb(QImage & image, quint32 & timestamp)
		{
			const int w = 640;
			const int h = 480;

			void *   data   = 0;
			uint32_t tstamp = 0;

			const int r = freenect_sync_get_video(&data, &tstamp, this->devIndex, FREENECT_VIDEO_RGB, 0);
			assert(r >= 0);
			if (r != 0) return false;

			// check disabled because timestamp overflows
			//if (quint32(tstamp) <= this->rgbLastTimestamp) return;
			this->rgbLastTimestamp = quint32(tstamp);
			timestamp = this->rgbLastTimestamp;

			image = QImage(w, h, QImage::Format_RGB888);
			memcpy(image.bits(), data, size_t(w * h * 3));

			return true;
		}

		bool acquireDepth(Shared::RawDepthMap & depthMap, quint32 & timestamp)
		{
			const int w = 640;
			const int h = 480;

			void *   data   = 0;
			uint32_t tstamp = 0;

			const int r = freenect_sync_get_depth(&data, &tstamp, this->devIndex, FREENECT_DEPTH_11BIT, 0);
			assert(r >= 0);
			if (r != 0) return false;

			// check disabled because timestamp overflows
			//if (quint32(tstamp) <= this->depthLastTimestamp) return;

			this->depthLastTimestamp = quint32(tstamp);
			timestamp = this->depthLastTimestamp;

			depthMap.setup(w, h, (const quint16 *)data);

			return true;
		}

		bool acquireAcceleration(Shared::Vec3f & acceleration, quint32 & timestamp)
		{
			freenect_raw_tilt_state * state = 0;

			int r = 0;
			r = freenect_sync_get_tilt_state(&state, this->devIndex);
			assert(r == 0);
			if (r != 0) return false;

			double acc[3] = { 0.0, 0.0, 0.0 };
			freenect_get_mks_accel(state, &(acc[0]), &(acc[1]), &(acc[2]));

			acceleration[0] = float(acc[0]);
			acceleration[1] = float(acc[1]);
			acceleration[2] = float(acc[2]);

			timestamp = 0;

			return true;
		}

	protected:

		int          devIndex;
		quint32      rgbLastTimestamp;
		quint32      depthLastTimestamp;
		quint32      accelLastTimestamp;
};

} // end namespace Kinect

} // end namespace WalkaWare

#endif // WALKAWARE_KINECT_DEVICE_H
